/**
 * @file    robot_base.cpp
 * @brief   Robot basic drive to communicate with controller
 * @author  Mr.Wu
 * @date    2025-05-28
 *
 * Copyright 2025 Mr.Wu
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "robot_base/robot_base.h"
#include <chrono>
#include <math.h>

using namespace std::chrono_literals;
using namespace std::placeholders;

namespace {
double imu_conversion(uint8_t data_h, uint8_t data_l) {
  int16_t value = (data_h << 8) | data_l;
  return value;
}

double degToRad(double deg) { return deg / 180.0 * M_PI; }
} // namespace

RobotBase::RobotBase(std::string name) : Node(name) {
  // 参数定义和获取
  this->declare_parameter("port_name", "ttyS1");
  this->get_parameter("port_name", port_name_);
  this->declare_parameter("baudrate", 115200);
  this->get_parameter("baudrate", baudrate_);
  this->declare_parameter("correct_factor_vx", 1.0);
  this->get_parameter("correct_factor_vx", correct_factor_vx_);
  this->declare_parameter("correct_factor_wz", 1.0);
  this->get_parameter("correct_factor_wz", correct_factor_wz_);
  this->declare_parameter("pub_odom_tf", false);
  this->get_parameter("pub_odom_tf", pub_odom_tf_);
  this->declare_parameter("use_imu", false);
  this->get_parameter("use_imu", use_imu_);
  this->declare_parameter("auto_stop_on", false);
  this->get_parameter("auto_stop_on", auto_stop_on_);

  // topic client初始化
  // 创建里程计、机器人状态的发布者
  odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
  if (use_imu_) {
    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
  }

  // topic subscriber初始化
  cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
      "cmd_vel", 10, std::bind(&RobotBase::cmd_vel_callback, this, _1));

  //开启通信串口
  try {
    serial_.setPort("/dev/" + port_name_); //制定打开的串口
    serial_.setBaudrate(baudrate_); // 波特率，即每秒钟读取的位数
    serial::Timeout timeout =
        serial::Timeout::simpleTimeout(2000); // 读取和写入超时时间2000ms
    serial_.setTimeout(timeout);
    serial_.open();
  } catch (serial::IOException &e) {
    RCLCPP_ERROR(
        this->get_logger(),
        "Open serial port failed:\n%s\nPlease check the serial port cable!",
        e.what());
  }
  if (serial_.isOpen()) {
    RCLCPP_INFO(this->get_logger(), "Robot serial port[%s] opened.",
                port_name_.c_str());
    // 开启一个新的线程负责串口数据读取
    std::thread{&RobotBase::_readData, this}.detach();
  }

  // IMU校准
  if (_imuCalibration()) {
    RCLCPP_INFO(this->get_logger(), "IMU calibration success!");
  } else {
    RCLCPP_ERROR(this->get_logger(), "IMU calibration failed!");
  }

  // 初始化定时器
  timer_ = this->create_wall_timer(100ms,
                                   std::bind(&RobotBase::timer_callback, this));

  // 启动完成之后，让buzzer响500ms
  _buzzerControl(true);
  usleep(500000);
  _buzzerControl(false); //关闭buzzer

  RCLCPP_INFO(this->get_logger(), "Robot started, welcome to enjoying it.");
}

RobotBase::~RobotBase() {
  //关闭串口
  serial_.close();
}

void RobotBase::_readData() {
  //读取，校验，处理
  uint8_t data;
  DataFrame frame;

  while (rclcpp::ok()) {
    auto len = serial_.read(&data, 1);
    if (len < 1) {
      continue; //读取失败，继续读取
    }

    if (data == 0x55) {
      //接收到帧头
      frame.header = data;
      serial_.read(&frame.id, 10);
      if (frame.tail != 0xBB) {
        RCLCPP_ERROR(this->get_logger(), "Receive frame error!");
        RCLCPP_INFO(this->get_logger(),
                    "Frame data[Error]: %02X %02X %02X %02X %02X %02X %02X "
                    "%02X %02X %02X %02X",
                    frame.header, frame.id, frame.length, frame.data[0],
                    frame.data[1], frame.data[2], frame.data[3], frame.data[4],
                    frame.data[5], frame.check, frame.tail);
        continue;
      }
      // 成功读取数据
      //校验
      if (_checkDataFrame(frame)) {
        //处理数据
        _processDataFrame(frame);
      } else {
        RCLCPP_ERROR(this->get_logger(), "Receive frame check error!");
      }
    }
  }
}

bool RobotBase::_checkDataFrame(DataFrame &frame) {
  //校验帧尾
  uint8_t check = 0;
  for (size_t i = 0; i < 6; i++) {
    check += frame.data[i];
  }
  return (check & 0xFF) == frame.check;
}

void RobotBase::_processDataFrame(DataFrame &frame) {
  //处理数据
  switch (frame.id) {
  case FRAME_ID_VELOCITY:
    _processVelocityData(frame);
    break;
  case FRAME_ID_ACCELERATION:
    _processAccelerationData(frame);
    break;
  case FRAME_ID_ANGULAR:
    _processAngularVelData(frame);
    break;
  case FRAME_ID_EULER:
    _processEulerData(frame);
    break;
  case FRAME_ID_SENSOR:
    _processSensorData(frame);
    break;
  default:
    RCLCPP_ERROR(this->get_logger(), "Receive frame id error: %02X", frame.id);
    break;
  }
}

void RobotBase::_transAngleInPI(float &angle) {
  if (angle > M_PI)
    angle -= 2 * M_PI;
  if (angle < -M_PI)
    angle += 2 * M_PI;
}
void RobotBase::_kinematicForward(float left_speed, float right_speed,
                                  float &linear_speed, float &angle_speed) {
  // 通过两侧轮子的速度，计算机器人整体的线速度和角速度，通过校正参数做校准
  linear_speed = correct_factor_vx_ * (left_speed + right_speed) / 2; // m/s
  angle_speed = correct_factor_wz_ * (right_speed - left_speed) /
                ROBOT_WHEEL_TRACK; // rad/s
}

void RobotBase::_odomAndTFPublish(float linear_speed, float angle_speed) {
  // 创建里程计消息并发布
  nav_msgs::msg::Odometry odom_msg;
  odom_msg.header.stamp = this->now();
  odom_msg.header.frame_id = "odom_msg"; // 里程计父坐标系

  odom_msg.child_frame_id =
      "base_footprint"; // 里程计子坐标系--机器人地盘坐标系

  // pose
  // 位置
  odom_msg.pose.pose.position.x = odom_.x;
  odom_msg.pose.pose.position.y = odom_.y;
  odom_msg.pose.pose.position.z = 0.0;
  // 姿态
  tf2::Quaternion q;
  q.setRPY(0, 0, odom_.theta);
  odom_msg.pose.pose.orientation.x = q.x();
  odom_msg.pose.pose.orientation.y = q.y();
  odom_msg.pose.pose.orientation.z = q.z();
  odom_msg.pose.pose.orientation.w = q.w();
  // 协方差
  // x和yaw方向位姿估计较高可靠性
  const double odom_pose_covariance[36] = {1e-3, 0,    0,   0,   0,   0,

                                           0,    1e-3, 0,   0,   0,   0,

                                           0,    0,    1e6, 0,   0,   0,
                                           0,    0,    0,   1e6, 0,   0,

                                           0,    0,    0,   0,   1e6, 0,

                                           0,    0,    0,   0,   0,   1e-9};
  // x和yaw方向位姿估计极高可靠性
  const double odom_pose_covariance2[36] = {
      1e-9, 0, 0, 0,   0, 0, 0, 1e-3, 1e-9, 0, 0,   0, 0, 0, 1e6, 0, 0, 0,
      0,    0, 0, 1e6, 0, 0, 0, 0,    0,    0, 1e6, 0, 0, 0, 0,   0, 0, 1e-9};

  // twist
  odom_msg.twist.twist.linear.x = linear_speed;
  odom_msg.twist.twist.linear.y = 0.0;
  odom_msg.twist.twist.linear.z = 0.0;
  odom_msg.twist.twist.angular.x = 0.0;
  odom_msg.twist.twist.angular.y = 0.0;
  odom_msg.twist.twist.angular.z = angle_speed;
  // 协方差
  // x和yaw方向速度估计较高可靠性
  const double odom_twist_covariance[36] = {1e-3, 0,    0,    0,   0,   0,
                                            0,    1e-3, 1e-9, 0,   0,   0,

                                            0,    0,    1e6,  0,   0,   0,

                                            0,    0,    0,    1e6, 0,   0,

                                            0,    0,    0,    0,   1e6, 0,

                                            0,    0,    0,    0,   0,   1e-9};
  // x和yaw方向速度估计极高可靠性
  const double odom_twist_covariance2[36] = {
      1e-9, 0, 0, 0,   0, 0, 0, 1e-3, 1e-9, 0, 0,   0, 0, 0, 1e6, 0, 0, 0,
      0,    0, 0, 1e6, 0, 0, 0, 0,    0,    0, 1e6, 0, 0, 0, 0,   0, 0, 1e-9};

  if (linear_speed == 0 && angle_speed == 0) {
    // 静止状态，pose和twist的测量数据都是极高可靠性
    memcpy(&odom_msg.pose.covariance, odom_pose_covariance2,
           sizeof(odom_pose_covariance2));
    memcpy(&odom_msg.twist.covariance, odom_twist_covariance2,
           sizeof(odom_twist_covariance2));
  } else {
    // 运动状态，pose和twist的测量数据都是使用高可靠性
    memcpy(&odom_msg.pose.covariance, odom_pose_covariance,
           sizeof(odom_pose_covariance));
    memcpy(&odom_msg.twist.covariance, odom_twist_covariance,
           sizeof(odom_twist_covariance));
  }
  // 发布里程计数据
  odom_pub_->publish(odom_msg);

  if (!pub_odom_tf_) {
    return;
  }

  // 发布tf动态广播
  geometry_msgs::msg::TransformStamped t;
  t.header.stamp = odom_msg.header.stamp;
  t.header.frame_id = "odom";
  t.child_frame_id = "base_footprint";
  t.transform.translation.x = odom_.x;
  t.transform.translation.y = odom_.y;
  t.transform.translation.z = 0.0;
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();
  tf_broadcaster_->sendTransform(t);
}

void RobotBase::_imuPublish() {
  // IMU话题数据填充和发布
  sensor_msgs::msg::Imu imu_msg;
  // header
  imu_msg.header.stamp = this->now();
  imu_msg.header.frame_id = "imu_link";

  // orientation
  tf2::Quaternion q;
  q.setRPY(imu_data_.roll, imu_data_.pitch, imu_data_.yaw);
  imu_msg.orientation.x = q.x();
  imu_msg.orientation.y = q.y();
  imu_msg.orientation.z = q.z();
  imu_msg.orientation.w = q.w();
  // orientation_covariance
  imu_msg.orientation_covariance = {0.0025, 0.0000, 0.0000, 0.0000, 0.0025,
                                    0.0000, 0.0000, 0.0000, 0.0025};

  // angular_velocity
  imu_msg.angular_velocity.x = imu_data_.angular_x;
  imu_msg.angular_velocity.y = imu_data_.angular_y;
  imu_msg.angular_velocity.z = imu_data_.angular_z;
  // angular_velocity_covariance
  imu_msg.angular_velocity_covariance = {0.02, 0.00, 0.00, 0.00, 0.02,
                                         0.00, 0.00, 0.00, 0.02};

  // linear_acceleration
  imu_msg.linear_acceleration.x = imu_data_.acceleration_x;
  imu_msg.linear_acceleration.y = imu_data_.acceleration_y;
  imu_msg.linear_acceleration.z = imu_data_.acceleration_z;
  // linear_acceleration_covariance
  imu_msg.linear_acceleration_covariance = {0.04, 0.00, 0.00, 0.00, 0.04,
                                            0.00, 0.00, 0.00, 0.04};

  if (use_imu_) {
    imu_pub_->publish(imu_msg);
  }
}

bool RobotBase::_imuCalibration() {
  DataFrame imu_frame{};
  imu_frame.header = 0x55;
  imu_frame.id = 0x07;
  imu_frame.length = 0x06;
  imu_frame.data[0] = 0x00;
  imu_frame.data[1] = 0x00;
  imu_frame.data[2] = 0x00;
  imu_frame.data[3] = 0x00;
  imu_frame.data[4] = 0xFF; // 使能控制IMU
  imu_frame.data[5] = 0xFF; // 校准IMU
  imu_frame.check =
      (imu_frame.data[0] + imu_frame.data[1] + imu_frame.data[2] +
       imu_frame.data[3] + imu_frame.data[4] + imu_frame.data[5]) &
      0xff;
  imu_frame.tail = 0xBB;

  try {
    serial_.write(&imu_frame.header, sizeof(imu_frame)); // 向串口发数据
  } catch (serial::IOException &e) {
    RCLCPP_ERROR(this->get_logger(),
                 "Unable to send data through serial port:\n%s",
                 e.what()); //如果发送数据失败,打印错误信息
    return false;
  }

  rclcpp::sleep_for(std::chrono::milliseconds(500)); // 等待IMU校准完成

  return true;
}

bool RobotBase::_buzzerControl(bool on) {
  DataFrame buzzer_frame{};
  buzzer_frame.header = 0x55;
  buzzer_frame.id = 0x07;
  buzzer_frame.length = 0x06;
  buzzer_frame.data[0] = 0x00;
  buzzer_frame.data[1] = 0x00;
  buzzer_frame.data[2] = 0xFF; //使能控制buzzer
  if (on) {
    buzzer_frame.data[3] = 0xFF;
  } else {
    buzzer_frame.data[3] = 0x00;
  }
  buzzer_frame.data[4] = 0x00;
  buzzer_frame.data[5] = 0x00;
  buzzer_frame.check =
      (buzzer_frame.data[0] + buzzer_frame.data[1] + buzzer_frame.data[2] +
       buzzer_frame.data[3] + buzzer_frame.data[4] + buzzer_frame.data[5]) &
      0xFF;
  buzzer_frame.tail = 0xBB;

  try {
    serial_.write(&buzzer_frame.header, sizeof(buzzer_frame)); // 向串口发数据
  } catch (serial::IOException &e) {
    RCLCPP_ERROR(this->get_logger(),
                 "Unable to send data through serial port:\n%s",
                 e.what()); //如果发送数据失败,打印错误信息
    return false;
  }

  return true;
}

void RobotBase::_processVelocityData(DataFrame &frame) {
  // 计算两个周期之间的时间差
  static rclcpp::Time last_time_ = this->now();
  current_time_ = this->now();
  float dt = (current_time_.seconds() - last_time_.seconds());
  last_time_ = current_time_;

  // 获取左右轮速度
  float left_speed =
      ((float)(frame.data[2] << 8 | frame.data[1])) / 1000.0; // m/s
  float right_speed =
      ((float)(frame.data[5] << 8 | frame.data[4])) / 1000.0; // m/s
  if (frame.data[0] == 0x00) {
    // 负数
    left_speed = -left_speed;
  }
  if (frame.data[3] == 0x00) {
    right_speed = -right_speed;
  }

  // 运动学正解
  float linear_speed, angle_speed;
  _kinematicForward(left_speed, right_speed, linear_speed, angle_speed);

  // 里程累计
  float delta_Vx;                    // 这个周期内X轴线速度变化量
  float delta_Vy;                    // 这个周期内Y轴线速度变化量
  float delta_th = angle_speed * dt; // 这个周期内角度变化量
  odom_.theta += delta_th;           // 角度累计
  delta_Vx = linear_speed * cos(odom_.theta) * dt; // 这个周期内X轴线速度变化量
  delta_Vy = linear_speed * sin(odom_.theta) * dt; // 这个周期内Y轴线速度变化量
  odom_.x += delta_Vx;                             // X轴线速度累计
  odom_.y += delta_Vy;                             // Y轴线速度累计

  // 角度大小限制
  _transAngleInPI(odom_.theta);

  //发布里程计数据和TF
  _odomAndTFPublish(linear_speed, angle_speed);
}

void RobotBase::_processAngularVelData(DataFrame &frame) {
  imu_data_.angular_x =
      imu_conversion(frame.data[1], frame.data[0]) / 32768 * degToRad(2000);
  imu_data_.angular_y =
      imu_conversion(frame.data[3], frame.data[2]) / 32768 * degToRad(2000);
  imu_data_.angular_z =
      imu_conversion(frame.data[5], frame.data[4]) / 32768 * degToRad(2000);
}

void RobotBase::_processAccelerationData(DataFrame &frame) {
  imu_data_.acceleration_x =
      imu_conversion(frame.data[1], frame.data[0]) / 32768 * 16 * 9.8;
  imu_data_.acceleration_y =
      imu_conversion(frame.data[3], frame.data[2]) / 32768 * 16 * 9.8;
  imu_data_.acceleration_z =
      imu_conversion(frame.data[5], frame.data[4]) / 32768 * 16 * 9.8;
}

void RobotBase::_processEulerData(DataFrame &frame) {
  imu_data_.roll =
      imu_conversion(frame.data[1], frame.data[0]) / 32768 * degToRad(180);
  imu_data_.pitch =
      imu_conversion(frame.data[3], frame.data[2]) / 32768 * degToRad(180);
  imu_data_.yaw =
      imu_conversion(frame.data[5], frame.data[4]) / 32768 * degToRad(180);

  if (use_imu_) {
    _imuPublish();
  }
}

void RobotBase::_processSensorData([[maybe_unused]] DataFrame &frame) {}

bool RobotBase::_velocityControl(float left_speed, float right_speed) {
  // 通过上下位机之间的串口通信,控制底盘运动
  // 速度控制的数据帧的 标识为：0x01
  // 数据帧封装
  DataFrame velocity_frame{};
  velocity_frame.header = 0x55;
  velocity_frame.id = 0x01;
  velocity_frame.length = 0x06;
  if (left_speed < 0) {
    velocity_frame.data[0] = 0x00; // 负数
  } else {
    velocity_frame.data[0] = 0xFF; // 正数
  }
  velocity_frame.data[1] =
      int(abs(left_speed) * 1000) & 0xFF; // cmd_vel:m/s--->mm/s
  velocity_frame.data[2] = (int(abs(left_speed) * 1000) >> 8) & 0xFF;
  if (right_speed < 0) {
    velocity_frame.data[3] = 0x00;
  } else {
    velocity_frame.data[3] = 0xFF;
  }
  velocity_frame.data[4] = int(abs(right_speed) * 1000) & 0xFF;
  velocity_frame.data[5] = (int(abs(right_speed) * 1000) >> 8) & 0xFF;
  velocity_frame.check = (velocity_frame.data[0] + velocity_frame.data[1] +
                          velocity_frame.data[2] + velocity_frame.data[3] +
                          velocity_frame.data[4] + velocity_frame.data[5]) &
                         0xFF;
  velocity_frame.tail = 0xBB;

  // 发送串口数据
  try {
    serial_.write(&velocity_frame.header, sizeof(velocity_frame));
  } catch (serial::IOException &e) {
    RCLCPP_ERROR(this->get_logger(),
                 "Unable to send velocity data through serial port:\n%s",
                 e.what()); //如果发送数据失败,打印错误信息
    return false;
  }
  return true;
}

void RobotBase::cmd_vel_callback(
    const geometry_msgs::msg::Twist::SharedPtr msg) {
  float linear_speed = msg->linear.x; // 通过cmd_vel给出的机器人整体线速度
  float angle_speed = msg->angular.z; // 通过cmd_vel给出的机器人整体角速度

  // 运动学逆解
  float left_speed = linear_speed - (ROBOT_WHEEL_TRACK * angle_speed) / 2.0;
  float right_speed = linear_speed + (ROBOT_WHEEL_TRACK * angle_speed) / 2.0;

  _velocityControl(left_speed, right_speed);
  // 开始自动更新计时，以便自动停车
  if (linear_speed != 0 || angle_speed != 0) {
    auto_stop_count_ = 0;
  }
}

void RobotBase::timer_callback() {
  // 自动停车
  if (auto_stop_on_ && auto_stop_count_ < 10) {
    auto_stop_count_++;

    // 超过500ms(500ms没没有速度控制指令),自动停止
    if (auto_stop_count_ > 5) {
      _velocityControl(0.0, 0.0);
      auto_stop_count_ = 50; // 给一个随机值，不再进入循环
    }
  }
}

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RobotBase>("robot_base");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}